
#include <stdio.h>
#include <errno.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdlib.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <memory>
#include "rtbus/can/canitf.h"

const uint8_t RETRY_CNT = 10;
const uint8_t WAIT_RESP_TIMEOUT = 10;

const uint8_t CAB_TYPE_MINICAB = 3;
const uint8_t CAB_TYPE_CAB2_1 = 2;

const uint16_t CAB_CAN_SDO_REQID = 0x612;
const uint16_t CAB_CAN_SDO_RESPID = 0x592;
const uint16_t CAB_CAN_OBJID = 0x214B;
const uint8_t CAB_COM_SEND_DATA[] = {0x01, 0x04, 0x00, 0x29, 0x00, 0x03, 0x61, 0xC3};

using namespace std;
using namespace rtbus;
shared_ptr<CANItf> can;
int ccan_init()
{
    auto drv = create_can(CANItf::CANType::SOCKETCAN);
    return -1;
}
int scan_init() { return -1; }
// int ecat_init() { return -1; }
static int com_init(int fd, int nSpeed, int nBits, char nEvent, int nStop)
{
    struct termios newttys1, oldttys1;
    /*保存原有串口配置*/
    if (tcgetattr(fd, &oldttys1) != 0)
    {
        return -1;
    }
    bzero(&newttys1, sizeof(newttys1));
    newttys1.c_cflag |= (CLOCAL | CREAD); /*CREAD 开启串行数据接收，CLOCAL并打开本地连接模式*/

    newttys1.c_cflag &= ~CSIZE; /*设置数据位*/
    /*数据位选择*/
    switch (nBits)
    {
    case 7:
        newttys1.c_cflag |= CS7;
        break;
    case 8:
        newttys1.c_cflag |= CS8;
        break;
    }
    /*设置奇偶校验位*/
    switch (nEvent)
    {
    case '0':                                 /*奇校验*/
        newttys1.c_cflag |= PARENB;           /*开启奇偶校验*/
        newttys1.c_iflag |= (INPCK | ISTRIP); /*INPCK打开输入奇偶校验；ISTRIP去除字符的第八个比特  */
        newttys1.c_cflag |= PARODD;           /*启用奇校验(默认为偶校验)*/
        break;
    case 'E':                                 /*偶校验*/
        newttys1.c_cflag |= PARENB;           /*开启奇偶校验  */
        newttys1.c_iflag |= (INPCK | ISTRIP); /*打开输入奇偶校验并去除字符第八个比特*/
        newttys1.c_cflag &= ~PARODD;          /*启用偶校验*/
        break;
    case 'N': /*无奇偶校验*/
        newttys1.c_cflag &= ~PARENB;
        break;
    }
    /*设置波特率*/
    switch (nSpeed)
    {
    case 2400:
        cfsetispeed(&newttys1, B2400);
        cfsetospeed(&newttys1, B2400);
        break;
    case 4800:
        cfsetispeed(&newttys1, B4800);
        cfsetospeed(&newttys1, B4800);
        break;
    case 9600:
        cfsetispeed(&newttys1, B9600);
        cfsetospeed(&newttys1, B9600);
        break;
    case 115200:
        cfsetispeed(&newttys1, B115200);
        cfsetospeed(&newttys1, B115200);
        break;
    default:
        cfsetispeed(&newttys1, B9600);
        cfsetospeed(&newttys1, B9600);
        break;
    }
    /*设置停止位*/
    if (nStop == 1) /*设置停止位；若停止位为1，则清除CSTOPB，若停止位为2，则激活CSTOPB*/
    {
        newttys1.c_cflag &= ~CSTOPB; /*默认为一位停止位； */
    }
    else if (nStop == 2)
    {
        newttys1.c_cflag |= CSTOPB; /*CSTOPB表示送两位停止位*/
    }

    /*设置最少字符和等待时间，对于接收字符和等待时间没有特别的要求时*/
    newttys1.c_cc[VTIME] = 0; /*非规范模式读取时的超时时间；*/
    newttys1.c_cc[VMIN] = 0;  /*非规范模式读取时的最小字符数*/
    tcflush(fd, TCIFLUSH);    /*tcflush清空终端未完成的输入/输出请求及数据；TCIFLUSH表示清空正收到的数据，且不读取出来 */

    /*激活配置使其生效*/
    if ((tcsetattr(fd, TCSANOW, &newttys1)) != 0)
    {
        printf("com set error\n");
        return -1;
    }

    return 0;
}
static int get_cab_version_by_com()
{
    printf("Try com...\n");
    int fd, nread, cab_type;
    unsigned char buff[512];

    /*cab 2.1 use ttyS2 for SCB*/
    fd = open("/dev/ttyS2", O_RDWR);
    if (-1 == fd)
    {
        printf("failed to open ttyS2 device.\n");
        return -1;
    }

    cab_type = -1;
    com_init(fd, 115200, 8, 'N', 1);
    nread = write(fd, (char*)CAB_COM_SEND_DATA, 8);
    usleep(200 * 1000);
    if ((nread = read(fd, (char*)buff, 512)) > 0)
    {
        printf("data received from ttyS2, size = %d\n", nread);
        for (int i = 0; i < nread; i++)
        {  //01 04 06 30 31 31 31 0 52 c6 69
            printf("%x ", buff[i]);
            if (i == nread - 1)
                printf("\n");
        }
        if (nread == 11 && buff[8] == 'R')
        {
            if (buff[7] == 'P')
            {
                cab_type = 3;  // Mini CAB
            }
            else
            {
                cab_type = 2;  // CAB V2.1
            }
        }
        else
        {
            cab_type = 0;  // Bootloader state
        }
    }
    else
    {
        printf("receive nothing from SCB\n");
        com_init(fd, 9600, 8, 'N', 1);
        cab_type = -1;
    }
    close(fd);
    return cab_type;
}

static int bus_init()
{
    printf("Try controlcan...\n");
    CANItf::Config cfg;
    cfg.adapter_name = "can1";
    cfg.dev_id = 1;
    can = create_can(CANItf::CANType::CONTROLCAN);
    can->load_cfg(cfg);
    if (!can.get() || can->active())
    {
        printf("Try socketcan...\n");
        can = create_can(CANItf::CANType::SOCKETCAN);
        can->load_cfg(cfg);
        if (!can.get() || can->active())
        {
            return -1;
        }
        printf("Fetch socketcan...\n");
    }
    else
    {
        printf("Fetch controlcan...\n");
    }

    return 0;
}

static void bus_exit()
{
    if (can.get())
    {
        can->deactive();
        can.reset();
    }
}

static int update_cab_version(shared_ptr<CANItf> can_)
{
    CANItf::FrameData send_frame;
    send_frame.id = CAB_CAN_SDO_REQID;
    send_frame.data[0] = 0xFF;
    send_frame.data[1] = CAB_CAN_OBJID >> 8 & 0xff;
    send_frame.data[2] = CAB_CAN_OBJID & 0xff;
    send_frame.dlc = 3;

    if (can_->send(&send_frame, 1) == 1)
    {
        for (int t = 0; t < WAIT_RESP_TIMEOUT; t++)
        {
            CANItf::FrameData recv_frame[100];
            memset(recv_frame, 0, sizeof(recv_frame));
            int recvCnt = can_->recv(recv_frame, 100);
            printf("\tFrame No.%d recvCnt: %d\n", t, recvCnt);
            for (int k = 0; k < recvCnt; k++)
            {
                if (recv_frame[k].id == CAB_CAN_SDO_RESPID)
                {
                    if (recv_frame[k].data[1] == (CAB_CAN_OBJID >> 8 & 0xff) && recv_frame[k].data[2] == (CAB_CAN_OBJID & 0xff))
                    {
                        return recv_frame[k].data[4] == 'P' ? CAB_TYPE_MINICAB : CAB_TYPE_CAB2_1;  // CAB2.1: 2, PSCB: 3
                    }
                }
            }
            usleep(8000);
        }
    }

    return -1;
}

static int get_cab_version_by_can()
{
    int trials = 0;
    int cab_version = -1;  // -1:error , 2: cab 2.1, 3 miniCAB

    while (trials++ < RETRY_CNT)
    {
        printf("Try %d\n", trials);
        if (bus_init() == 0)
        {
            cab_version = update_cab_version(can);
            if (cab_version > 0)
                break;
            bus_exit();
        }
        usleep(1000 * 200);
    }

    printf("cab version: %d\n", cab_version);
    return cab_version;
}

int main()
{
    auto cab = get_cab_version_by_com();
    if (cab > 0)
        return cab;
    return get_cab_version_by_can();
}
